package roborally;

import java.awt.IllegalComponentStateException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.NavigableMap;
import java.util.Random;
import java.util.Set;
import java.util.TreeSet;

import be.kuleuven.cs.som.annotate.*;
import roborally.Exceptions.IllegalAmountException;
import roborally.Exceptions.IllegalDestinationException;

/**
 * A class of robots, extending Placeable and implementing IRobot, for playing the roborally game, involving an x - and y-position, an orientation, an energy level,
 * a maximum amount of energy, possessions, costs to turn, to move, a standard weight unit and a standard energy unit.
 *
 * @invar  The orientation has to be valid.
 *         | Orientation.isValidOrientation(this.getOrientation())
 * @invar  The amount of energy a robot has, can never be negative nor be greater than its maximum energy.
 * 		   | Energy.isValidEnergyAmount(this.getEnergy(),this.getStandardEnergyUnit(),this.getMaxEnergy())
 * @invar  The maxEnergy of a robot, can never be negative nor be greater than 20000 WS.
 * 		   | Energy.isValidMaxEnergyAmount(this.getMaxEnergy(),DOUBLE.MAX_VALUE)
 * @invar  The cost to turn for a robot should never be smaller than zero.
 * 		   | isValidCostToTurn(this.getCostToTurn())
 * @invar  The cost to move for a robot should never be smaller than zero.
 * 		   | isValidCostToMove(this.getCostToMove())
 * 
 * @version  2.90
 * @author   Michiel Ruelens & Dieter Naten
 */

public class Robot extends Placeable implements IRobot  {


	private Orientation orientation;
	private Energy energy;
	private Energy maxEnergy;
	public final double standardMaxEnergyAmountInWS = 20000;
	public static final int StandardCostToTurn = 100;
	public static final int StandardCostToMove = 500;
	public static final int StandardCostToShoot = 1000;
	public int costToTurn = 100;
	public int costToMove = 500;
	public int costToShoot = 1000;
	private final EnergyUnit standardEnergyUnit = EnergyUnit.WATTSECOND;
	private final WeightUnit standardWeightUnit = WeightUnit.GRAM;
	private HashMap<PlaceableType,Set<Carryable>> possessions;
	private TreeSet<Carryable> possessionsOrderedByWeight;
	Comparator<Carryable> comparator;
	private Program program;
	
	
	/**
	 * Initialize the new robot with given parameters.
	 * 
	 * @param 	board
	 * 		    The board the robot is going to be put on.
	 * @param   x
     * 		    The x-position of the new robot.
     * @param   y
     * 		    The y-position of the new robot.
     * @param   orientation
     * 		    The orientation of the new robot.
     * @param   energy
     *          The energy of the new robot.
     * @param   maxEnergy
     *          The maxEnergy of the new robot.
     * @pre     The amount of given maxEnergy should be valid.
     * 			|  Energy.isValidMaxEnergyAmount(newMaxEnergy,Double.MAX_VALUE)
     * @pre     The amount of given energy should be valid.
     * 			|  Energy.isValidEnergyAmount(energy,this.getStandardEnergyUnit(),this.getMaxEnergy())
     * @effect  The board of this new robot is set.
     * 			| setBoard(board)
     * @effect  The orientation of this new robot is set.
     * 			| setOrientation(Orientation.convertIntToOrientation(orientation))
     * @effect  The placeabletype of this new robot is set.
     * 			| setPlaceableType(PlaceableType.ROBOT)
     * @effect  The maxEnergy of this new robot is set.
     * 			| setMaxEnergy(new Energy(maxEnergy,standardEnergyUnit))
     * @effect  The energy of this new robot is set.
     * 			| setEnergy(new Energy(energy,standardEnergyUnit))
     * @effect  The coordinate of this new robot is set.
     * 			| setCoordinate(x,y)
     * @effect  The robot is added to the given board.
     * 			| board.addPlaceable(this)
	 */
	@Raw
	public Robot(Board board, long x, long y, int orientation, double energy, double maxEnergy)	{
		super();
		setBoard(board);
		if(Orientation.isValidOrientation(Orientation.convertIntToOrientation(orientation))){
			setOrientation(Orientation.convertIntToOrientation(orientation));
		}
		else{
			System.out.println("The given orientation isn't valid!");
			setOrientation(Orientation.convertIntToOrientation(1));
		}
		Energy newMaxEnergy = new Energy(maxEnergy,standardEnergyUnit);
		assert(Energy.isValidMaxEnergyAmount(newMaxEnergy,Double.MAX_VALUE));
		setMaxEnergy(newMaxEnergy);
		assert(Energy.isValidEnergyAmount(energy, standardEnergyUnit, getMaxEnergy()));
		setEnergy(new Energy(energy,standardEnergyUnit));
		setCoordinate(x,y);
		setPlaceableType(PlaceableType.ROBOT);
		possessions = new HashMap<PlaceableType,Set<Carryable>>();
		comparator = getComparator();
		possessionsOrderedByWeight = new TreeSet<Carryable>(comparator);
		board.addPlaceable(this);
	}

	/**
     * Initialize the new robot with given orientation and energy.
     * 
     * @param   orientation
     * 		    The orientation of the new robot.
     * @param   energy
     *          The energy of the new robot.
     * @pre	    The given energy must be a valid energy for a robot.
     *          | Energy.isValidEnergyAmount(energy, standardEnergyUnit, getMaxEnergy())
     * @effect  The orientation of this new robot is set.
     * 			| setOrientation(Orientation.convertIntToOrientation(orientation))
     * @effect  The placeabletype of this new robot is set.
     * 			| setPlaceableType(PlaceableType.ROBOT)
     * @effect  The maxEnergy of this new robot is set.
     * 			| setMaxEnergy(new Energy(maxEnergy,standardEnergyUnit))
     * @effect  The energy of this new robot is set.
     * 			| setEnergy(new Energy(energy,standardEnergyUnit))
     */
	@Raw
	public Robot(int orientation, double energy) {
		super();
		setPlaceableType(PlaceableType.ROBOT);
		if(Orientation.isValidOrientation(Orientation.convertIntToOrientation(orientation))){
			setOrientation(Orientation.convertIntToOrientation(orientation));
		}
		else{
			System.out.println("The given orientation isn't valid!");
			setOrientation(Orientation.convertIntToOrientation(1));
		}
		setMaxEnergy(new Energy(20000,standardEnergyUnit));
		assert(Energy.isValidEnergyAmount(energy, standardEnergyUnit, getMaxEnergy()));
		setEnergy(new Energy(energy,standardEnergyUnit));
		possessions = new HashMap<PlaceableType,Set<Carryable>>();
		comparator = getComparator();
		possessionsOrderedByWeight = new TreeSet<Carryable>(comparator);
		comparator = getComparator();
	}
	
	/**
	 * Puts the robot on the desired board at the desired coordinate(x,y).
	 * 
	 * @param 	board
	 * 		    The board the robot is going to be put on.
	 * @param   x
     * 		    The x-position of the new robot.
     * @param   y
     * 		    The y-position of the new robot.
	 * @effect	| putPlaceable(board,x,y)
	 */
	//putPlaceable vangt de exceptions op die van de setX en setY methodes kunnen komen.
	public void putRobot(Board board, long x,long y){
		putPlaceable(board,x,y);		
	}
	
	/**
	 * This method will return the current orientation of the robot.
	 */
	@Override
	@Basic
	public int getOrientation() {
		return Orientation.convertOrientationToInt(orientation);
	}
	
	/**
	 * This method sets the orientation of the robot to the requested orientation. 
	 * 
	 * @param  direction
	 * 		   The orientation to which the robot should be moved.
	 * @post   The new orientation of the robot is equal to direction.
	 * 		   | if(Orientation.isValidOrientation(direction)
	 *         | 	(new this).getOrientation() == direction
	 *         | else
	 *         |	(new this).getOrientation() == getOrientation()
	 */
	@Raw
	@Model
	private void setOrientation(Orientation direction){
		if(!Orientation.isValidOrientation(direction)) { 
			System.out.println("That is not a valid orientation."); 
		}
		else{ 
			this.orientation = direction; 
		}
	}

    /**
     * Returns the cost to shoot for a robot.
     */
	@Basic
	public int getCostToShoot() {
		return costToShoot;
	}

    /**
     * Returns the cost to turn for a robot.
     */
	@Basic
	public int getCostToTurn() {
		return costToTurn;
	}
	
	/**
	 * Sets the costToTurn for a robot to the given parameter.
	 * 
	 * @param costToTurn
	 *        The parameter to which the costToTurn should be set
	 * @pre	  The given costToTurn must be valid.
	 * 		  | isValidCostToTurn(costToTurn)
	 * @post  The costToTurn is equal to the given parameter
	 *        | (new this).getCostToTurn() == costToTurn
	 */
	public void setCostToTurn(int costToTurn) {
		assert(isValidCostToTurn(costToTurn));
		this.costToTurn = costToTurn;
	}

	/**
	 * Checks whether the given cost is a valid cost to turn.
	 * This is the case when the cost is greater or equal to zero.
	 * 
	 * @param 	cost
	 * 			The parameter that is being validated.
	 * @return	True if the given cost is greater than or equal to zero
	 * 			| result ==
	 * 			| (cost>=0)
	 */
	@Model
	private boolean isValidCostToTurn(int cost){
		return (cost>=0);
	}
	
    /**
     * Returns the cost to move for a robot, depending on the weight of its carryables.
     */
	@Basic
	public int getCostToMove() {
		return costToMove;
	}

	/**
	 * Sets the costToMove for a robot to the given parameter.
	 * 
	 * @param costToMove
	 *        The parameter to which the costToMove should be set
	 * @pre	  The given costToMove must be valid.
	 * 		  | isValidCostToMove(costToMove)
	 * @post  The costToMove is equal to the given parameter
	 *        | (new this).getCostToMove() == costToMove
	 */
	public void setCostToMove(int costToMove) {
		assert(isValidCostToMove(costToMove));
		this.costToMove = costToMove;
	}
	
	/**
	 * Checks whether the given cost is a valid cost to move.
	 * This is the case when the cost is greater or equal to zero.
	 * @param 	cost
	 * 			The parameter that is being validated.
	 * @return	True if the given cost is greater than or equal to zero
	 * 			| result ==
	 * 			| (cost>=0)
	 */
	@Model
	private boolean isValidCostToMove(int cost){
		return (cost>=0);
	}

	/**
	 * Returns the extra energy a robot needs every step when it has picked op carryables.
	 * 
	 * @return	| for(Carryable c: carryableList)
	 * 			|	 result += (c.getWeight().toUnit(Unit.GRAM).getNumeral()*50)/1000
	 */
	@Model
	private int getExtraEnergyFromWeight(){
//		Set<? extends Carryable> batteryList = getAllPossessionsByType(PlaceableType.BATTERY);
		Set<Carryable> carryableList = new HashSet<Carryable>();
		carryableList.addAll(getAllPossessions());
		int result = 0;
		if(!carryableList.isEmpty()){
			for(Carryable c: carryableList){
					Weight weight = c.getWeight();
					weight.toWeightUnit(WeightUnit.GRAM);
					int numeral = weight.getNumeral();
					result = result+(numeral*50)/1000;
			}
		}
		return result;		
	}
	
	/**
	 * This method will return the current energy of the robot.
	 * 
	 * @return  The energy of the current robot, numeral given.
	 * 			| result ==
	 * 			| energy.getNumeral()
	 */
	@Override
	public double getEnergy() {
		return energy.getNumeral();
	}
	
	/**
	 * The following method returns the energy of this robot as an Energy object (including Unit).
	 * It's weird name stems from the fact that we need to override getEnergy from IRobot to return
	 * the numeral value of the energy in watt-second.
	 */
	@Basic
	public Energy getEnergyObject() {
		return energy;
	}
	
	/**
	 * This method sets the energy of the robot to the requested energy. 
	 * 
	 * @param  energy
	 * 		   The energy which the robot should get.
	 * @pre    The amount of energy must be valid.
     *         | Energy.isValidEnergyAmount(energy.getNumeral(),energy.getUnit(),getMaxEnergy())  
	 * @post   The new energy of the robot is equal to energy. 
	 *         | (new this).getEnergy() == energy
	 */
	@Raw
	public void setEnergy(Energy energy) {
		assert(Energy.isValidEnergyAmount(energy.getNumeral(),energy.getUnit(),getMaxEnergy()));
		this.energy = energy;
	}
	 
	/**
	 * This method recharges the robot with a given amount of energy expressed in WATTSECOND.
	 * 
	 * @param 	energyAmount
	 * 			The amount of energy you want to add to the robot expressed in WATTSECOND.
	 * @pre  	The added amount of energy must be valid, given an initial energy-level
     * 			and a maximum possible energy-level for the robot.
     *          | isAddableEnergyAmount(energyAmount, getMaxEnergy().getNumeral(), this.getEnergy())  
     * @post  	The energy of the robot is the sum of the previous
     * 			energy of the robot and the energyAmount to add.
     *          | (new this).getEnergy() = new Energy(getEnergy()+energyAmount,Unit.WATTSECOND)
	 */
	@Override
	public void recharge(double energyAmount) {
		assert(isAddableEnergyAmount(energyAmount, getMaxEnergy().getNumeral(), this.getEnergy()));
		//same units (WATTSECOND)
		if(energy.getUnit()==EnergyUnit.WATTSECOND){	
			energy = new Energy(getEnergy()+energyAmount,EnergyUnit.WATTSECOND);
		}
		//different units (Ws-kWh)=> first convert, then add.
		else{
			Energy convertedEnergyToAdd = new Energy(energyAmount,EnergyUnit.WATTSECOND).toUnit(energy.getUnit());
			energy = new Energy(getEnergy()+convertedEnergyToAdd.getNumeral(),energy.getUnit());
		}
	}
	
	/**
     * Check if it is possible to add a certain amount of energy to the robot, given an initial energy-level
     * and a maximum possible energy-level for the robot.
     *  
     * @param  energyAmount
     * 		   The amount of energy you want to add to the robot.
     * @param  maxEnergy
     * 		   The maximum amount of energy the robot can have.
     * @param  energy
     * 		   The amount of energy the robot initially has.
     * @return True if the given energyAmount is not negative 
     *         and if the initial energy-level plus the energyAmount do no exceed the maximum allowed energy.
     *         | result ==
     *         | (energyAmount >= 0) && (energyAmount <= (maxEnergy - energy))	
     */
	public static boolean isAddableEnergyAmount(double energyAmount, double maxEnergy, double energy) {
		return (energyAmount >= 0) && (energyAmount <= (maxEnergy - energy));
	 }
	
	/**
	 * This method will return the maxEnergy of the robot.
	 */
	@Basic
	public Energy getMaxEnergy() {
		return maxEnergy;
	}
	
	/**
	 * This method sets the maxEnergy of the robot to the requested maxEnergy. 
	 * 
	 * @param  maxEnergyAmount	
	 * 		   The maxEnergy which the robot should get.
	 * @pre    The amount of maxEnergy must be valid.
     *         | Energy.isValidMaxEnergyAmount(maxEnergyAmount,Double.MAX_VALUE))
	 * @post   The new maxEnergy of the robot is equal to maxEnergyAmount. 
	 *         | (new this).getMaxEnergy() == maxEnergyAmount
	 */
	@Raw
	public void setMaxEnergy(Energy maxEnergyAmount) {
		assert(Energy.isValidMaxEnergyAmount(maxEnergyAmount,Double.MAX_VALUE));
		this.maxEnergy = maxEnergyAmount;
	}

	/**
	 * This method will return the current fraction energy of the robot.
	 * 
	 * @return The current fraction energy of the robot.
	 * 		 | result == 
	 * 		 | (energy.getNumeral()/getMaxEnergy().getNumeral())*100			
	 */
	@Override
	public double getFractionEnergy() {
		if(energy.getUnit()==maxEnergy.getUnit()){
			return (int) (energy.getNumeral()/getMaxEnergy().getNumeral())*100;
		}
		else{
			Energy maxEnergyInSameUnit = maxEnergy.toUnit(energy.getUnit());
			return (int) (energy.getNumeral()/maxEnergyInSameUnit.getNumeral())*100;
		}
	}

	/**
	 * This method calculates the energy required for a robot to move to a certain position.
	 * 
	 * @param 	x
	 * 			The x-coordinate where the robot should move to.
	 * @param 	y
	 * 			The y-coordinate where the robot should move to.
	 * @pre  	The values of x and y must be valid coordinates. 
     *         	| Coordinate.isValidXCoordinate(board,x)
     *         	| Coordinate.isValidYCoordinate(board, y)
	 * @return  The energy required to reach a certain position, by following the given path.
	 * 		  	| result ==
	 * 			| pathList.get(pathList.size()-2).getEnergyToReach()
	 * 			| && (pathList == (ArrayList<Node>) shortestPathAlgorithm.getShortestPath(this, Coordinate.getCoordinate(x,y), 
	 *			|	  Orientation.convertIntToOrientation(getOrientation()), getEnergy(),
	 *			|	  getBoard()))
	 * @return	If the robot does not have sufficient energy to reach that coordinate the method returns -2.
	 * 			| if(this.getEnergy()<energyRequiredToReach)
	 *			| 	result == -1;
	 * @return 	If the robot is not located on any board, the method returns -1.
	 * 			| if(this.getBoard()==null)
     *			|	result == -1;
     * @return	If the destination could not be reached because of obstacles, this method returns -1.
     * 			| if(targetNode.getCoordinate().getX()!=x || targetNode.getCoordinate().getY()!=y)
	 *			|	result == -1
	 * @throws 	IllegalArgumentException
	 * 		   	The given x and/or y coordinate is not valid.
	 *         	| (!Coordinate.isValidXCoordinate(board,x) || !Coordinate.isValidYCoordinate(board, y))
	 */
	@Override
	public double getEnergyRequiredToReach(long x, long y) throws IllegalArgumentException{
    if(!Coordinate.isValidXCoordinate(board,x) || !Coordinate.isValidYCoordinate(board, y)){
    	throw new IllegalArgumentException();
    }
    	if(this.getBoard()==null){
    		return -1;
    	}
		List<Node> pathList = (ArrayList<Node>) shortestPathAlgorithm.getShortestPath(this, Coordinate.getCoordinate(x,y), 
				Orientation.convertIntToOrientation(getOrientation()), getEnergy(),
				getBoard());
		double energyRequiredToReach = 0;
		Node targetNode = pathList.get(pathList.size()-1);
		energyRequiredToReach = targetNode.getEnergyToReach();
		if(targetNode.getCoordinate().getX()!=x || targetNode.getCoordinate().getY()!=y){
			energyRequiredToReach = -1;
		}
		else if(this.getEnergy()<energyRequiredToReach){
			energyRequiredToReach = -1;
		}
		return energyRequiredToReach;
	}
	 
	/**
	 * This method returns whether the implementations of getEnergyRequiredToReach() and moveNextTo()
	 * take into account the fact that turning consumes energy (required to score 16+).
	 * 
	 * @return  If getEnergyRequiredToReach() and moveNextTo() take in account that 
	 * 		    turning consumes energy.
	 * 		 	| result ==
	 * 		  	| 1
	 * @return  If getEnergyRequiredToReach() and moveNextTo() don't take in account that 
	 * 		    turning consumes energy.
	 * 		  	| result ==
	 * 		  	| 0
	 */
	@Override
	public int isGetEnergyRequiredToReachAndMoveNextTo16Plus() {
		return 1;
	}
	
	
	/**
	 * Terminate this robot and all its possessions.
	 * 
	 * @effect	| for(Carryable c: getAllPossessions())
	 * 			|		this.removeCarryableFromPossesions(c)
	 * 			|		c.terminate()
	 * @effect	| this.possessionsOrderedByWeight.clear()
	 * @effect	| super.terminate()
	 */
	@Override
	public void terminate(){
		if(!this.isTerminated()){
			//Terminate robots possessions
			List<? extends Carryable> possessions = this.getAllPossessions();
			if(!possessions.isEmpty()){
				for(Carryable c:possessions){
					this.removeCarryableFromPossessions(c);
					c.terminate();
				}
			}
			this.possessionsOrderedByWeight.clear();
			super.terminate();
		}
		else{
			System.out.println("That robot has already been terminated.");
		}
	}
	
	/**
	 * This method returns the i'th heaviest possession of the robot.
	 * This means that the 1st heaviest possessions (i==1) returns the heaviest possession that this robot is carrying.
	 * 
	 * @param 	i
	 * 			The i'th heaviest carryable is returned.
	 * @pre		The parameter entered must be valid, this is it must be greater than zero.
	 * 			| (i>0)
	 * @return	The i'th heaviest item
	 * 			| result ==
	 * 			| possessionsOrderedByWeight.toArray[i-1]
	 */
	public Carryable getIthHeaviestPossession(int i){
		assert(i>0);
		 ArrayList<Carryable> carryableList = new ArrayList<Carryable>(possessionsOrderedByWeight);
		 Carryable carryable = (Carryable) carryableList.get(i-1);
		return carryable;
	}
	
	/**
	 * Returns all possessions of this robot.
	 * 
	 * @return	A list containing all the possessions of this robot.
	 *          | result ==
	 *          | possessions
	 */
	public List<? extends Carryable> getAllPossessions(){
		ArrayList<Carryable> resultSet = new ArrayList<Carryable>();
		Set<PlaceableType> typeSet = new HashSet<PlaceableType>();
		if(possessions.keySet() != null){
			typeSet = possessions.keySet();
		}
		else{
			System.out.println("There are no possessions.");
		}
		for(PlaceableType type : typeSet){
			resultSet.addAll(possessions.get(type));
		}
		return resultSet;
	}
	
	/**
	 * Get all possessions of this robot of the specified type
	 * 
	 * @param 	type
	 * 			The type of which all possessions should be returned
	 * @return	All possessions of the robot of the specified type
	 *          | result == possessions.get(type)
	 */
	public Set<? extends Carryable> getAllPossessionsByType(PlaceableType type){
		Set<Carryable> resultSet = new HashSet<Carryable>();
		if(possessions.containsKey(type)){
			resultSet.addAll(possessions.get(type));
		}
		return resultSet;
	}
	
	/**
	 * Make the robot pick up a carryable that is at the same position as the robot.
	 * If successful the carryable is added to the robots possessions.
	 * 
	 * @param 	carryable
	 * 			The carryable that is being picked up.
	 * @effect	The carryStatus of the carryable has been set to true
	 *          | carryable.setCarryStatus(true)
	 * @effect	The carryable has been added to the robots possessions.
	 * 			| addCarryableToPossessions(carryable)
	 * @effect	The carryable has been removed from the board.
	 * 			| board.removePlaceable(carryable)
	 * @effect	The board of the carryable has been set to null.
	 * 			| carryable.setBoard(null);
	 */
	public void pickUp(Carryable carryable){
		if(!this.isTerminated()){
			if(!carryable.isTerminated()){
				if(carryable.getX()==this.getX()&&carryable.getY()==this.getY()){
					if(carryable.getBoard() == this.getBoard()){
					addCarryableToPossessions(carryable);
					carryable.setCarryStatus(true);
					this.getBoard().removePlaceable(carryable);
					carryable.setBoard(null);
					}
					else{
						System.out.println("You must be standing on the same board as the item you want to pick up.");
					}
				}
				else{
					System.out.println("You must be standing in the same place as the item you want to pick up.");
				}
			}
			else{
				System.out.println("The item you are trying to pick up has been destroyed.");
			}
		}
		else{
			System.out.println("This robot cannot do anything as it has been destroyed.");
		}
	}
	
	/**
	 * Add the carryable to the robots possessions.
	 * This method also adjusts the cost to move for a robot, cause it has a possession less.
	 * 
	 * @param 	carryable
	 * 			The carryable that is being added.
	 * @pre 	The given carryable shouldn't be terminated.
	 * 			| !carryable.isTerminated()
	 * @post	The carryable has been added to the robots list of possessions
	 *          | possessions.getAllPossessionsByType(carryable.getType()).contains(carryable) == true
	 * @effect 	The cost to move for the robot is adjust to the new value with one more carryable.
	 * 			| this.setCostToMove(StandardCostToMove + getExtraEnergyFromWeight())
	 */
	public void addCarryableToPossessions(Carryable carryable){
		assert(!carryable.isTerminated());
		Set<Carryable>	typeSet = new HashSet<Carryable>();
		//Get the old set if it exists
		if(possessions.containsKey(carryable.getPlaceableType())){
			typeSet = possessions.get(carryable.getPlaceableType());
		}
		//Add the carryable
		typeSet.add(carryable);
		//Put the new set back in, overwriting the old one
		possessions.put(carryable.getPlaceableType(), typeSet);
		possessionsOrderedByWeight.add(carryable);
		this.setCostToMove(StandardCostToMove + getExtraEnergyFromWeight());
		}
	
	/**
	 * Use a carryable that is being carried.
	 * 
	 * @param 	carryable
	 * 			The carryable that should be used.
	 * @effect	| carryable.useCarryable(this)
	 */
	public void useCarryable(Carryable carryable){
		if(carryable.isTerminated()){
			this.removeCarryableFromPossessions(carryable);
		}
		//Robot must possess carryable
		else if(getAllPossessionsByType(carryable.getPlaceableType()).contains(carryable)){
				//Carryable must be useable
				if(carryable.isUseable()){
				carryable.useCarryable(this);
				}
				else{
					System.out.println("That item is not useable.");
				}
		}
		else{
			System.out.println("This robot does not possess that item.");
		}
	}
	
	/**
	 * This method drops a carryable the robot was holding and leaves it on the board
	 * at the position where the robot is.
	 * 
	 * @param  	carryable
	 *         	The Carryable to be dropped.
	 * @effect 	The carryable has been removed from the robots possessions.
	 * 			| removeCarryableFromPossessions(carryable)
	 * @effect 	The carryable is placed on the same board as the robot that dropped it.
	 *         	| carryable.setBoard(this.getBoard())
	 * @effect  The carryable is placed at the same coordinates as the robot that dropped it. 
	 *         	| carryable.setCoordinate(this.getX(), this.getY())
	 * @effect  The carryable isn't carried anymore
	 *         	| carryable.getCarryStatus() == false
	 * @effect  The carryable is added on the board.
	 *         	| this.getBoard().addPlaceable(carryable)
	 */
	public void drop(Carryable carryable){
		if(getAllPossessionsByType(carryable.getPlaceableType()).contains(carryable)){
		removeCarryableFromPossessions(carryable);
		carryable.setBoard(getBoard());
		carryable.setCoordinate(getX(), getY());
		carryable.setCarryStatus(false);
		this.getBoard().addPlaceable(carryable);
		}
		else {
			System.out.println("This carryable : " + carryable + " isn't a part of your possessions!");
		}
	}
	
	/**
	 * Removes a carryable that should be dropped from the possessions set of a robot.
	 * This method also adjusts the cost to move for a robot, cause it has a possession less.
	 * 
	 * @param 	carryable
	 *         	The carryable that should be removed from the possessions set
	 * @post   	The robot no longer possesses the carryable
	 *         	| (new this).getAllPossesions.contains(carryable) == false
	 * @effect 	The cost to move for the robot is adjust to the new value with one more carryable.
	 * 			| this.setCostToMove(StandardCostToMove + getExtraEnergyFromWeight())
	 */
	public void removeCarryableFromPossessions(Carryable carryable){
		Set<Carryable>	typeSet = new HashSet<Carryable>();
		//Get the old set if it exists
		if(possessions.containsKey(carryable.getPlaceableType())){
			typeSet = possessions.get(carryable.getPlaceableType());
			//Remove the carryable
			typeSet.remove(carryable);
			//If the set is empty clear possessions of that type
			if(typeSet.isEmpty()){
				possessions.remove(carryable.getPlaceableType());
			}
			//Otherwise put the remaining items back in, overwriting the old set
			else{
				possessions.put(carryable.getPlaceableType(), typeSet);
			}
			possessionsOrderedByWeight.remove(carryable);
		}
		else{
			System.out.println("There is no such item to remove.");
		}
		this.setCostToMove(StandardCostToMove + getExtraEnergyFromWeight());
	}
	
	/**
	 * This method will add a certain amount to the X-coordinate of the robot. 
	 * This means that the robot will move horizontally towards the right side of the board.
	 * 
	 * @param  amount
	 * 		   The amount to add to the X-coordinate of the robot.
	 * @effect The new X-coordinate of the robot is equal to the old X-coordinate plus amount. 
	 *         | this.setCoordinate(coordinate.getX()+amount,coordinate.getY())
	 * @throws IllegalAmountException
	 * 		   The given amount is not a valid amount to add.
	 *         | !Coordinate.isValidXCoordinate(this.getBoard(), this.getX()+amount)
	 */
	public void addX(long amount) throws IllegalAmountException {
		if(!Coordinate.isValidXCoordinate(board, this.getX()+amount)){
			throw new IllegalAmountException((Robot) this,amount, "x");
		}
		this.setCoordinate(coordinate.getX()+amount,coordinate.getY());
		
	}
	
	/**
	 * This method will subtract a certain amount from the X-coordinate of the robot. 
	 * This means that the robot will move horizontally towards the left side of the board.
	 * 
	 * @param  amount
	 * 		   The amount to subtract from the X-coordinate of the robot.
	 * @effect The new X-coordinate of the robot is equal to the old X-coordinate minus amount. 
	 *         | this.setCoordinate(coordinate.getX()-amount,coordinate.getY())
	 * @throws IllegalAmountException
	 * 		   The given amount is not a valid amount to subtract.
	 *         | !Coordinate.isValidXCoordinate(this.getBoard(), this.getX()-amount)
	 */
	@Model
	public void subtractX(long amount) throws IllegalAmountException{
		if(!Coordinate.isValidXCoordinate(board, this.getX()-amount)){
			throw new IllegalAmountException((Robot) this,-amount, "x");
		}
		this.setCoordinate(coordinate.getX()-amount,coordinate.getY());
	}
	
	/**
	 * This method will add a certain amount to the Y-coordinate of the robot. 
	 * This means that the robot will move vertically towards the bottom of the board.
	 * 
	 * @param  amount
	 * 		   The amount to add to the Y-coordinate of the robot.
	 * @effect The new Y-coordinate of the robot is equal to the old Y-coordinate plus amount. 
	 *         | this.setCoordinate(coordinate.getX(),coordinate.getY()+amount)
	 * @throws IllegalAmountException
	 * 		   The given amount is not a valid amount to add.
	 *         | !Coordinate.isValidYCoordinate(this.getBoard(), this.getY()+amount)
	 */
	@Model
	public void addY(long amount) throws IllegalAmountException{
		if(!Coordinate.isValidYCoordinate(board, this.getY()+amount)){
			throw new IllegalAmountException((Robot) this,amount, "y");
		}
		this.setCoordinate(coordinate.getX(),coordinate.getY()+amount);
	}
	
	/**
	 * This method will subtract a certain amount from the Y-coordinate of the robot. 
	 * This means that the robot will move vertically towards the top side of the board.
	 * 
	 * @param  amount
	 * 		   The amount to subtract from the Y-coordinate of the robot.
	 * @effect The new Y-coordinate of the robot is equal to the old Y-coordinate minus amount. 
	 *         | this.setCoordinate(coordinate.getX(),coordinate.getY()-amount)
	 * @throws IllegalAmountException
	 * 		   The given amount is not a valid amount to subtract.
	 *         | !Coordinate.isValidYCoordinate(this.getBoard(), this.getCoordinate.getY-amount)
	 */
	public void subtractY(long amount) throws IllegalAmountException{
		if(!Coordinate.isValidYCoordinate(board, this.getY()-amount)){
			throw new IllegalAmountException((Robot) this,-amount, "y");
		}
		this.setCoordinate(coordinate.getX(),coordinate.getY()-amount);
	}
	
	/**
	 * This method will turn the robot clockwise and will update the energy accordingly, if the robot has sufficient energy.
	 * 
	 * @post	If the robot turns, the energy is lowered by costToTurn.
	 * 		    | this.energy = new Energy(energy.toUnit(Unit.WATTSECOND).getNumeral()-costToTurn,standardEnergyUnit).toUnit(energy.getUnit())
	 * @effect	The robot turned clockwise.
	 *          | (new this).getOrientation() = Orientation.getNextOrientationClockwise(this.get(Orientation())
	 */
	@Override
	public void turnClockwise() {
		if(this.getEnergy() >= getCostToTurn()){
			orientation = Orientation.getNextOrientationClockwise(orientation);
			EnergyUnit oldUnit = energy.getUnit();
			this.energy = new Energy(energy.toUnit(EnergyUnit.WATTSECOND).getNumeral()-getCostToTurn(),standardEnergyUnit).toUnit(oldUnit);
			}
		else{ 
			System.out.println("Your robot does not have sufficient energy to turn.");
		}
	}
	
	/**
	 * This method will turn the robot anti-clockwise and will update the energy accordingly, if the robot has sufficient energy.
	 * 
	 * @post	If the robot turns, the energy is lowered by costToTurn.
	 * 		    | this.energy = new Energy(energy.toUnit(Unit.WATTSECOND).getNumeral()-costToTurn,standardEnergyUnit).toUnit(energy.getUnit())
	 * @effect  The robot turned anti-clockwise.
	 *          | (new this).getOrientation() = Orientation.getNextOrientationAntiClockwise(this.get(Orientation())
	 */
	public void turnAntiClockwise() {
		if(this.getEnergy() >= getCostToTurn()){
			orientation = Orientation.getNextOrientationAntiClockwise(orientation);
			EnergyUnit oldUnit = energy.getUnit();
			this.energy = new Energy(energy.toUnit(EnergyUnit.WATTSECOND).getNumeral()-getCostToTurn(),standardEnergyUnit).toUnit(oldUnit);
			}
		else{ 
			System.out.println("Your robot does not have sufficient energy to turn.");
		}
	}

	/**
	 * This method will turn the robot clockwise several times, equal to amount and will update the energy accordingly.
	 * This will only happen when the robot has sufficient energy to make all the requested turns.
	 * 
	 * @effect 	The robot turned clockwise several times.
	 *		  	| for(int i=0; i<amount; i++)
	 *		  	|  	--> turnClockwise()
	 */
	public void turnClockwise(int amount) {
		if(this.getEnergy() >= getCostToTurn()*amount){
			for(int i=0; i<amount; i++){ 
				turnClockwise(); 
			}
		}
		else{ 
			System.out.println("Your robot does not have sufficient energy to make all the requested turns.");
		}
	}
	
	/**
	 * This method will turn the robot anti-clockwise several times, equal to amount and will update the energy accordingly.
	 * This will only happen when the robot has sufficient energy to make all the requested turns.
	 * 
	 * @effect 	The robot turned anti-clockwise several times.
	 *		 	| for(int i=0; i<amount; i++)
	 *		  	| 	--> turnAntiClockwise()
	 */
	public void turnAntiClockwise(int amount) {
		if(this.getEnergy() >= getCostToTurn()*amount){
			for(int i=0; i<amount; i++){	
				turnAntiClockwise(); 	
			}
		}
		else{ 
			System.out.println("Your robot does not have sufficient energy to make all the requested turns.");
		}
	}

	
	/**
	 * This method will move the robot 1 step in the direction it is currently facing.
	 * 
	 * @pre     energy.toUnit(Unit.WATTSECOND).getNumeral()>=costToMove
	 * @post    The energy of the robot is lowered by costToMove if the robot has moved.
	 * 		    | this.energy = new Energy(energy.toUnit(Unit.WATTSECOND).getNumeral()-(costToMove),standardEnergyUnit).toUnit(energy.getUnit())
	 * @effect  When the robot is facing orientation '0', has sufficient energy and there's no wall above it,
	 *          the robot is moved 1 step to the top.
	 * 		    | if (getOrientation()==0)
	 *          |    if(!board.hasWallAtPosition(this.getX(),this.getY()-1))
	 * 		    |		subtractY(1);
	 * @effect  When the robot is facing orientation '1', has sufficient energy and there's no wall above it,
	 *          the robot is moved 1 step to the right.
	 * 		    | if (getOrientation()==1)
	 *          |    if(!board.hasWallAtPosition(this.getX()+1,this.getY()))
	 * 		    |		addX(1);
	 * @effect  When the robot is facing orientation '2', has sufficient energy and there's no wall above it,
	 *          the robot is moved 1 step down.
	 * 		    | if (getOrientation()==2)
	 *          |    if(!board.hasWallAtPosition(this.getX(),this.getY()+1))
	 * 		    |		addY(1);
	 * @effect  When the robot is facing orientation '3', has sufficient energy and there's no wall above it,
	 *          the robot is moved 1 step to the left.
	 * 		    | if (getOrientation()==3)
	 *          |    if(!board.hasWallAtPosition(this.getX()-1,this.getY()))
	 * 		    |		subtractX(1);
	 * @throws	IllegalStateException
	 * 			Thrown if this robot does not have a board.
	 * 			| if(this.getBoard()==null
	 * @throws  IllegalDestinationException
	 *          Thrown if the x- or y-coordinate isn't valid.
	 * 		    | if(!Coordinate.isValidXCoordinate(board,x) || !Coordinate.isValidYCoordinate(board,y))
	 * @throws  IllegalArgumentExeception
	 *          Thrown if there's a wall at the position (with coordinate: (moveToX,moveToY)) to which the robot wants to move.
	 *          | if(board.hasWallAtPosition(moveToX,moveToY)
	 */
	@Override
	public void move() throws IllegalDestinationException, IllegalArgumentException {
//		System.out.println(getEnergy());
		assert(energy.toUnit(EnergyUnit.WATTSECOND).getNumeral()>=getCostToMove());
		if(this.getBoard()==null){
			throw new IllegalStateException();
		}
		boolean containsWall=false;
		Coordinate oldCoordinate = this.getCoordinate(); //get the old coordinate
		//System.out.println(oldCoordinate.getX()+" "+oldCoordinate.getY());
		if(getOrientation() == 0){ 
			if(!board.hasWallAtPosition(this.getX(),this.getY()-1)){ 
				subtractY(1); 
			}
			else{
				containsWall = true;
			}
		}
		else if(getOrientation() == 1){ 
			if(!board.hasWallAtPosition(this.getX()+1,this.getY())){ 
				addX(1); 	
			} 
			else{
				containsWall = true;
			}
		}
		else if(getOrientation() == 2){ 
			if(!board.hasWallAtPosition(this.getX(),this.getY()+1)){ 
				addY(1);	
			} 
			else{
				containsWall = true;
			}
		}
		else if(getOrientation() == 3){ 
			if(!board.hasWallAtPosition(this.getX()-1,this.getY())){ 
				subtractX(1); 
			} 
			else{
				containsWall = true;
			}
		}
		if(!containsWall){
			EnergyUnit oldUnit = energy.getUnit();
			this.energy = new Energy(energy.toUnit(EnergyUnit.WATTSECOND).getNumeral()-(costToMove),standardEnergyUnit).toUnit(oldUnit);
			Coordinate newCoordinate = this.getCoordinate(); //get the new coordinate
			//System.out.println(newCoordinate.getX()+" "+newCoordinate.getY());;
			this.board.updateHashMapByPositionAfterMove(this, oldCoordinate, newCoordinate);//update the boards hashmap with the robots new position
		}
		else{
			throw new IllegalArgumentException();
		}
//		System.out.println("After: "+getEnergy());
	}

	
	/**
	 * Move one step to an adjacent coordinate.
	 * 
	 * @param	c1
	 * 			The adjecent coordinate where the robot should move to.
	 * @effect 	| if(c1.getX()>getX() && orientation == Orientation.RIGHT
	 *		   	| 	OR c1.getX()<getX() && orientation == Orientation.LEFT
	 *   	   	| 	OR c1.getY()<getY() && orientation == Orientation.TOP
	 *			| 	OR c1.getY()>getY() && orientation == Orientation.BOTTOM)
	 *			|	--> 	move()
	 * @effect 	| if(c1.getX()>getX() && orientation == Orientation.LEFT
	 *			| 	OR c1.getX()<getX() && orientation == Orientation.RIGHT
	 *			| 	OR c1.getY()<getY() && orientation == Orientation.BOTTOM
	 *			| 	OR c1.getY()>getY() && orientation == Orientation.TOP) {
	 *			|	--> 	turnClockwise(2)
	 *			|	--> 	move()
	 * @effect  | if(c1.getX()>getX() && orientation == Orientation.TOP
	 *			| 	OR c1.getX()<getX() && orientation == Orientation.BOTTOM
	 *			| 	OR c1.getY()<getY() && orientation == Orientation.LEFT
	 *			| 	OR c1.getY()>getY() && orientation == Orientation.RIGHT) {
	 *			|	--> 	turnClockwise()
	 *			|	--> 	move()
	 * @effect  | if(c1.getX()>getX() && orientation == Orientation.BOTTOM
	 *			| 	OR c1.getX()<getX() && orientation == Orientation.TOP
	 *			| 	OR c1.getY()<getY() && orientation == Orientation.RIGHT
	 *			| 	OR c1.getY()>getY() && orientation == Orientation.LEFT) {
	 *			|	--> 	turnAntiClockwise()
	 *			|	-->		move()
	 */
	public void moveOneStep(Coordinate c1){
		if(c1.getX()>getX() && orientation == Orientation.RIGHT
				|| c1.getX()<getX() && orientation == Orientation.LEFT
				|| c1.getY()<getY() && orientation == Orientation.TOP
				|| c1.getY()>getY() && orientation == Orientation.BOTTOM) {
			move();
		}
		else if(c1.getX()>getX() && orientation == Orientation.LEFT
				|| c1.getX()<getX() && orientation == Orientation.RIGHT
				|| c1.getY()<getY() && orientation == Orientation.BOTTOM
				|| c1.getY()>getY() && orientation == Orientation.TOP) {
			turnClockwise(2);
			move();
		}
		else if(c1.getX()>getX() && orientation == Orientation.TOP
				|| c1.getX()<getX() && orientation == Orientation.BOTTOM
				|| c1.getY()<getY() && orientation == Orientation.LEFT
				|| c1.getY()>getY() && orientation == Orientation.RIGHT) {
			turnClockwise();
			move();
		}
		else if(c1.getX()>getX() && orientation == Orientation.BOTTOM
				|| c1.getX()<getX() && orientation == Orientation.TOP
				|| c1.getY()<getY() && orientation == Orientation.RIGHT
				|| c1.getY()>getY() && orientation == Orientation.LEFT) {
			turnAntiClockwise();
			move();
		}
	}
	
	
	/**
	 * Method that moves the 2 given robots next to eachother
	 * 
	 * @param 	robot
	 *        	The first of two robots you want to move next to each other
	 * @param 	robot2
	 *        	The second of two robots you want to move next to each other
	 * @post  	If the robots had sufficient energy to move next to each other, they're standing next to each other.
	 *			Coordinate.standNextToEachOther(robot.getCoordinate(),robot2.getCoordinate()) == true
	 * @throws	IllegalArgumentException
	 *			When the 2 robots are not on the same board
	 *			| if(r1.getBoard()!=r2.getBoard())
	 * @throws	IllegalComponentStateException()
	 * 			When one of the robots is terminated.
	 * 			| if(r1.isTerminated() || r2.isTerminated())
	 */
	@Override
	public void moveNextTo(IRobot robot, IRobot robot2) {
		Robot r1 = (Robot) robot;
		Robot r2 = (Robot) robot2;
		if(r1.isTerminated() || r2.isTerminated()){
			System.out.println("One of the robots is terminated so you cannot move them next to each other.");
			throw new IllegalComponentStateException();
		}
		shortestPathAlgorithm.clearAlgorithm();
		Node.clearNodeList();
		boolean done = false;
		boolean done2 = false;
		if(r1.getBoard()!=r2.getBoard()){
			throw new IllegalArgumentException();
		}
		else if(Coordinate.standNextToEachOther(r1.getCoordinate(),r2.getCoordinate())){
			System.out.println("These robots are already standing next to each other.");
		}
		else if(r1.getCoordinate().equals(r2.getCoordinate())){
				List<Node> pathList11 = shortestPathAlgorithm.moveOneStepBecauseOnTop(r1);
				done = makeRobotWalkAlongPathTowardsOtherRobot(r1,r2,pathList11);
				done2 = false;
				if(!done){
					shortestPathAlgorithm.clearAlgorithm();
					Node.clearNodeList();
					List<Node> pathList12 = shortestPathAlgorithm.moveOneStepBecauseOnTop(r2);
					done2 = makeRobotWalkAlongPathTowardsOtherRobot(r1,r2,pathList12);
				}
		}
		else{
			List<Node> pathList21 = (ArrayList<Node>) shortestPathAlgorithm.getShortestPath(r1, r2.getCoordinate(), 
												Orientation.convertIntToOrientation(r1.getOrientation()), r1.getEnergy(),
												r1.getBoard());
			done = makeRobotWalkAlongPathTowardsOtherRobot(r1,r2,pathList21);
			done2 = false;
			if(!done){
				shortestPathAlgorithm.clearAlgorithm();
				Node.clearNodeList();
				List<Node> pathList22 = (ArrayList<Node>) shortestPathAlgorithm.getShortestPath(r2, r1.getCoordinate(), 
													Orientation.convertIntToOrientation(r2.getOrientation()), r2.getEnergy(),
													r2.getBoard());
				done2 = makeRobotWalkAlongPathTowardsOtherRobot(r2,r1,pathList22);
			}
			shortestPathAlgorithm.clearAlgorithm();
			Node.clearNodeList();
		}
		if(done || done2){
			System.out.println("Target successfully reached.");
		}
	}
	
	/**
	 * Makes a robot walk along the path, if it reaches its destination it returns true, otherwise returns false
	 * 
	 * @param 	robot
	 * 		  	The robot that has to walk towards the other robot
	 * @param 	otherRobot
	 *        	The robot to which the other robot should walk
	 * @param 	pathList
	 * 		  	List with the path of nodes the walking robot has to follow
	 * @return  If the robot reached it's destination
	 * 			| result ==
	 * 			| true
	 * @return  If the robot didn't reach it's destination
	 * 			| result==
	 * 			| false
	 */
	public boolean makeRobotWalkAlongPathTowardsOtherRobot(Robot robot,Robot otherRobot, List<Node> pathList){
		boolean destinationReached = false;
		
//		System.out.println("Robot Coords: "+robot.getCoordinate().getX()+" "+robot.getCoordinate().getY());
//		System.out.println("targetRobot Coords: "+otherRobot.getCoordinate().getX()+" "+otherRobot.getCoordinate().getY());
		int i = 1;
		boolean done = false;
		boolean outOfEnergy1 = false;
		if(pathList.size()>1){
			while(i<pathList.size() && done==false && outOfEnergy1==false){
//				System.out.println(i);
//				System.out.println(pathList.size());
//				System.out.println("pre "+robot.getEnergy());
				if(pathList.get(i)!=null && robot.getEnergy()<(pathList.get(i).getEnergyToReach()-pathList.get(i-1).getEnergyToReach())){
					outOfEnergy1 = true;
//					System.out.println("true");
				}
				else{
					System.out.println("Robots coordinate: "+robot.getCoordinate().getX()+" "+robot.getCoordinate().getY());
					robot.moveOneStep(pathList.get(i).getCoordinate());
					if(Coordinate.standNextToEachOther(robot.getCoordinate(), otherRobot.getCoordinate())){
						done=true;
						destinationReached=true;
					}
				}
				i++;
//				System.out.println("post "+robot.getEnergy());
			}
		}
		return destinationReached;
	}

	/**
	 * Allows a robot to shoot.
	 * 
	 * @effect If board has a placeable in the direction the robot shoots,
	 *         the first placeable (with coordinate == coordinateShot) that is met, will be hit.
	 *         | board.shootPlaceableAt(coordinateShot.getX(),coordinateShot.getY());
	 */
	public void shoot(){
		if(this.getEnergy()>= 1000){
			Coordinate coordinateShot = Coordinate.getCoordinateInDirectionThatContainsPlaceable(Orientation.convertIntToOrientation(this.getOrientation()), this.getBoard(), this);
			if(coordinateShot != null){
				board.shootPlaceableAt(coordinateShot.getX(),coordinateShot.getY());
			}
			else{
				System.out.println("There is nothing to be shot.");
			}
			Energy newEnergy = new Energy((this.getEnergy()-costToShoot),this.getEnergyObject().getUnit());
			setEnergy(newEnergy);
		}
		else{
			System.out.println("Your robot hasn't got enough energy to shoot!");
		}
	}
	
	/**
	 * Method executed when a robot is hit.
	 * 
	 * @effect	The robots maxEnergy is reduced with 4000 WS, if possible. If maxEnergy becomes lower than zero, the robot is destroyed.
	 * 			|currentMaxEnergy = this.getMaxEnergy().toUnit(EnergyUnit.WATTSECOND).getNumeral()
	 * 			| 	if(currentMaxEnergy-4000<=0) {this.terminate()	}
	 * 			|	else if(currentMaxEnergy-4000>0) {
	 * 			|		this.setMaxEnergy(currentMaxEnergy-4000,EnergyUnit.WATTSECOND)
	 * 			|		if(this.getEnergy()>(currentMaxEnergy-4000)){
	 * 			|			this.setEnergy(new Energy(currentMaxEnergy-4000,EnergyUnit.WATTSECOND)))
	 * 			|		}
	 * 			|	}
	 */
	@Override
	public void hit(){
		double currentMaxEnergy = this.getMaxEnergy().toUnit(EnergyUnit.WATTSECOND).getNumeral();
		if(currentMaxEnergy-4000<=0){
			this.terminate();
			System.out.println("A robot has been killed, cause it got hit! GAME OVER!");
		}
		else if(currentMaxEnergy-4000>0){
			if(this.getEnergy()>(currentMaxEnergy-4000)){
				Energy newEnergy = new Energy(currentMaxEnergy-4000,EnergyUnit.WATTSECOND);
				this.setEnergy(newEnergy);
			}
			Energy newMaxEnergy = new Energy(currentMaxEnergy-4000,EnergyUnit.WATTSECOND);
			this.setMaxEnergy(newMaxEnergy);
			System.out.println("Your robot has been hit, it has lost 4000 maxEnergy.");
			System.out.println("It's maxEnergy now equals " + this.getMaxEnergy().toUnit(EnergyUnit.WATTSECOND).getNumeral() + " Ws.");
		}
	}

	/**
	 * This method transfers carryables from the current robot to the given robot, if they're standing next to eachother.
	 * 
	 * @param	to
	 * 			The robot to which the items should be transfered
	 * @effect 	The carryables from the first robot are transfered to the other robot
	 * 			| for(Carryable carryable: this.getAllPossessions())
	 * 			|	this.removeCarryableFromPossessions(carryable)
	 * 			|	to.addCarryableToPossessions(carryable)
	 */
	public void transfersCarryables(Robot to) {
		if(Coordinate.standNextToEachOther(this.getCoordinate(), to.getCoordinate()))
		{
			List<? extends Carryable> CarryableList = this.getAllPossessions();
			if(!CarryableList.isEmpty()){
				for(Carryable carryable: CarryableList){
					this.removeCarryableFromPossessions(carryable);
					carryable.putPlaceable(to.getBoard(), to.getX(), to.getY());
					to.addCarryableToPossessions(carryable);
				}
			}
			else{
				System.out.println("This robot doesn't possess any item!");
			}
		}
		else{
			System.out.println("The given robots aren't standing next to each other!");
		}
	}
	
	/**
	 * This method returns the program of the robot
	 */
	@Basic
	public Program getProgram() {
			return program;
	}
	
	/**
	 * Sets the program of this robot to loadedProgram. To replace an existing program with a new one,
	 * the old one needs to be removed first by calling setProgram(null).
	 * 
	 * @param 	loadedProgram
	 * 			The program the robot should get
	 * @post	The robot should get the new program 	
	 * 			| (new this).getProgram == loadedProgram
	 */
	public void setProgram(Program loadedProgram) {
		if(loadedProgram==null || this.getProgram()==null){
			this.program = loadedProgram;
		}
		else{
			System.out.println("That robot already has a program, please remove it first.");
		}
		
	}
	
	/**
	 * Load a program from the given file-path.
	 * 
	 * @param 	path
	 * 			The path from which the program should be loaded
	 * @return  An integer indicating if the loading worked
	 * 			| if(Program.loadProgramFromFile(path)!=null)
	 * 			| result ==
	 * 			| 0 
	 * @effect	If the program isn't null, it is set to be the new program
	 * 			| if(Program.loadProgramFromFile(path)!=null)
	 * 			| this.setProgram(loadedProgram)
	 * @return  An integer indicating if the loading worked
	 * 			| if(Program.loadProgramFromFile(path)==null)
	 * 			| result ==
	 * 			| -1 
	 * 			
	 */
	public int loadProgramFromFile(String path){
		int successful = -1;
		Program loadedProgram = Program.loadProgramFromFile(path);
		if(loadedProgram!=null){
			this.setProgram(loadedProgram);
			successful = 0;
		}
		else{
			System.out.println("Failed to load program.");
		}
		return successful;
	}

	/**
	 * Save this robots program to the given path.
	 * 
	 * @param 	path
	 * 		  	The path where the program should be saved
	 * @effect	An integer indicating if the loading was succesfull is returned
	 * 			| Program.saveProgramToFile(this.program,path)
	 */
	public int saveProgramToFile(String path){
		int successful = Program.saveProgramToFile(this.program,path);
		return successful;
	}
	
	/**
	 * Execute on step of this robots program. That is, execute the program until a basic command is executed.
	 * Basic commands are: move, turn, shoot, pickup-and-drop. The next call of this method will start at the
	 * statement directly following the last executed basic command.
	 * 
	 * @effect  One step of a program is exectued
	 * 			| Program.executeOneStepOfProgram(this, this.program)
	 */
	public void executeOneStepOfProgram(){
		Program.executeOneStepOfProgram(this, this.program);
	}
	
	/**
	 * Creates a new comparator that compares 2 weights.
	 * The comparator returns:
	 * 
	 * @return	A comparator that will compare weights
	 */
    private Comparator<Carryable> getComparator() {
    	return new Comparator<Carryable>() {
    		
    		/**
    		 * Compares 2 weights.
    		 * @param 	w1
    		 * 			The first weight
    		 * @param	w2
    		 * 			The second weight
    		 * @return	Returns -1 if w1<w2 in the same units.
    		 * 			Returns 1 if w1>w2 in the same units.
    		 *			Returns 0 if w1==w2 in the same units.
    		 */
		    public int compare(Carryable w1, Carryable w2) {
		        int weight1 = w1.getWeight().toWeightUnit(standardWeightUnit).getNumeral();
		        int weight2 = w2.getWeight().toWeightUnit(standardWeightUnit).getNumeral();
//		        int result = 0;
//		        if(weight1<weight2){
//		        	result = -1;
//		        }
//		        else if(weight1==weight2){
//		        	result = 1;
//		        }
//		        return result;
		        return  (weight1 > weight2 ? 1 : (weight1 == weight2 ? 0 : -1));
		    }
		};
	}
}